#include "dead_reckoning.h"
static double g_record_locate_x = 0;    //上一时刻的全局记录坐标X
static double g_record_locate_y = 0;    //上一时刻的全局记录坐标y
static double g_record_angle = 0;
/*坐标系为以机体上电为原点，轮轴中心为精确原点，前进方向为Y轴正方向，右手为X轴正方向
Y^
 |
 |
 |
 |
 |
 |
O _ _ _ _ _ _ _ _>
                X
*/

/*
* @description: pulse differnce convert to distance difference
* @param:    pulse differnce
* @return: distance difference
*/
double xp_pulse_differnce_to_distance(int pulse_value_differ)
{
    double circle_num =  (((double)(pulse_value_differ))/MATH_PULSE_PR);
    return (circle_num/MATH_WHEEL_MOTOR_REDUCE_RATIO)*MATH_PI*MATH_WHEEL_DIAMETER;
}

/*
* @description: by using GGPM01LK module angle getted convert to radian unit
    and radiant must be respect to the X-axis
* @param:    angle:-180 180 
* @return: distance difference  

*/
double xp_angle_to_radian_by_ggpm01lk(double angle)
{
    double angle_agv_body = 0;
    if(fabs(angle - 0)<GREAT_SMALL_POINT_NUMBER)
    {
        angle_agv_body = 90.0;
    }
    else if(fabs(angle - 90.0)<GREAT_SMALL_POINT_NUMBER)
    {
        angle_agv_body = 0.0;
    }
    else if(fabs(angle - 180.0)<GREAT_SMALL_POINT_NUMBER)
    {
        angle_agv_body = -90.0;
    }
    else if(fabs(angle - (-180.0))<GREAT_SMALL_POINT_NUMBER)
    {
        angle_agv_body = -90.0;
    }
    else if(fabs(angle - (-90.0))<GREAT_SMALL_POINT_NUMBER)
    {
        angle_agv_body = 180.00;
    }
    else if(angle >0 && angle <90)
    {
        angle_agv_body = 90 - angle;
    }
    else if(angle > 90 && angle <180)
    {
        angle_agv_body = 90 -angle;
    }
    else if(angle <0 && angle >-90)
    {
        angle_agv_body = 90-angle;
    }
    else if(angle >-180 && angle <-90)
    {
        angle_agv_body = -angle - 270;
    }
    //printf("angle_agv_body %f \n ",angle_agv_body);
    return angle_agv_body *MATH_PI/180.00;
}
 
 
 /*
 * @description: calculate the location coordinate
 * @param:   
 left_pulse_differ:left wheel encode difference 
 right_pulse_differ:right  wheel encode difference 
 angle:Notice that this Angle right here is the Angle with respect to the X-axis
 location_x:need to calculate resault 
 location_y:need to calculate resault 
 @return:
        0 success
        -1 fail
 */

 int  xp_calculate_location_coordinate(int left_pulse_differ,int right_pulse_differ,double angle,double *location_x,double *location_y)
{
    double left_wheel_dis = 0;
    double right_wheel_dis = 0;
    double average_wheel_dis = 0;
    double angle_radian =0;
    if((location_x == NULL) ||(location_y == NULL))
    {
        printf("location_x is NULL");
        return -1;
    }
    if((abs(left_pulse_differ)<10)&&(abs(right_pulse_differ)<10))
    {
       return -1;
    }
    //printf(">>>>left_pulse_differ %d right_pulse_differ %d \n",left_pulse_differ,right_pulse_differ);
    left_wheel_dis = xp_pulse_differnce_to_distance(left_pulse_differ);
    right_wheel_dis = xp_pulse_differnce_to_distance(right_pulse_differ);
    average_wheel_dis = (left_wheel_dis + right_wheel_dis)/2.00;   
    //printf("left_wheel_dis  is  %f  right_wheel_dis is  %f average_wheel_dis %f\n",left_wheel_dis,right_wheel_dis,average_wheel_dis);
    angle_radian = xp_angle_to_radian_by_ggpm01lk(angle);
    *location_x = g_record_locate_x + average_wheel_dis * cos(angle_radian);
    *location_y = g_record_locate_y + average_wheel_dis * sin(angle_radian);
    g_record_locate_x = *location_x;
    g_record_locate_y = *location_y;
    //printf("left_wheel_dis %f  right_wheel_dis %f  x :%f y:%f \n",left_wheel_dis,left_wheel_dis,g_record_locate_x,g_record_locate_y);
    return 0;
}
 
/*
* @description: get last location X coordinate
* @param:void 
* @return: void
*/
 double xp_get_last_location_x()
 {
    return g_record_locate_x;
 }
 
 /*
* @description: get last location Y coordinate
* @param:void 
* @return: void
*/
 double xp_get_last_location_y()
 {
    return g_record_locate_y;
 }
/*
* @description: reset last record location information
* @param:void 
* @return: void
*/

void xp_clear_record_location()
{
    g_record_locate_x = 0;
    g_record_locate_y = 0;
}


